{
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "intro_md"
      },
      "source": [
        "# PoseTranslationPrior"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "desc_md"
      },
      "source": [
        "`PoseTranslationPrior<POSE>` is a unary factor that applies a prior constraint only to the **translation** component of a `POSE` variable (e.g., `Pose2` or `Pose3`).\n",
        "It ignores the rotation component of the pose variable during error calculation.\n",
        "The error is calculated as the difference between the translation component of the pose variable and the measured prior translation, expressed in the tangent space (which is typically just vector subtraction for `Point2` or `Point3`).\n",
        "\n",
        "This is useful when you have information about the absolute position of a pose but little or no information about its orientation (e.g., GPS measurement)."
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "colab_badge_md"
      },
      "source": [
        "<a href=\"https://colab.research.google.com/github/borglab/gtsam/blob/develop/gtsam/slam/doc/PoseTranslationPrior.ipynb\" target=\"_parent\"><img src=\"https://colab.research.google.com/assets/colab-badge.svg\" alt=\"Open In Colab\"/></a>"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {
        "id": "pip_code",
        "tags": [
          "remove-cell"
        ]
      },
      "outputs": [],
      "source": [
        "%pip install --quiet gtsam-develop"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 2,
      "metadata": {
        "id": "imports_code"
      },
      "outputs": [],
      "source": [
        "import gtsam\n",
        "import numpy as np\n",
        "from gtsam import Pose3, Rot3, Point3, Values, PoseTranslationPrior3D\n",
        "from gtsam import symbol_shorthand\n",
        "\n",
        "X = symbol_shorthand.X"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_header_md"
      },
      "source": [
        "## Creating a PoseTranslationPrior"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "create_desc_md"
      },
      "source": [
        "Provide the key of the pose variable, the measured prior translation (`Point3` for `Pose3`, `Point2` for `Pose2`), and a noise model defined on the translation space dimension (e.g., 3 for `Point3`)."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 4,
      "metadata": {
        "id": "create_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "PoseTranslationPrior: PoseTranslationPrior  keys = { x0 }\n",
            "isotropic dim=3 sigma=0.5\n",
            "Measured Translation[\n",
            "\t10;\n",
            "\t20;\n",
            "\t5\n",
            "]\n"
          ]
        }
      ],
      "source": [
        "pose_key = X(0)\n",
        "measured_translation = Point3(10.0, 20.0, 5.0) # Prior belief about position\n",
        "\n",
        "# Noise model on translation (3 dimensions for Point3)\n",
        "translation_noise = gtsam.noiseModel.Isotropic.Sigma(3, 0.5) # 0.5 meters std dev\n",
        "\n",
        "# Factor type includes the Pose type, e.g. PoseTranslationPriorPose3\n",
        "factor = PoseTranslationPrior3D(pose_key, measured_translation, translation_noise)\n",
        "factor.print(\"PoseTranslationPrior: \")\n",
        "\n",
        "# Alternative constructor: extract translation from a full Pose3 prior\n",
        "full_pose_prior = Pose3(Rot3.Yaw(np.pi/2), measured_translation) # Rotation is ignored\n",
        "factor_from_pose = PoseTranslationPrior3D(pose_key, full_pose_prior, translation_noise)\n",
        "# factor_from_pose.print(\"\\nFrom Pose Prior: \") # Should be identical"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_header_md"
      },
      "source": [
        "## Evaluating the Error"
      ]
    },
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "eval_desc_md"
      },
      "source": [
        "The error depends only on the translation part of the `Pose3` value in the `Values` object."
      ]
    },
    {
      "cell_type": "code",
      "execution_count": 5,
      "metadata": {
        "id": "eval_example_code"
      },
      "outputs": [
        {
          "name": "stdout",
          "output_type": "stream",
          "text": [
            "Error with correct translation: 0.0 (Should be near zero)\n",
            "Error with incorrect translation: 0.11999999999999986 (Should be non-zero)\n",
            "Error with different rotation: 0.11999999999999986 (Should reflect Jacobian change)\n",
            "Unwhitened error with different rotation: [ 0.2 -0.1  0.1] (Should be [0.2, -0.1, 0.1])\n"
          ]
        }
      ],
      "source": [
        "values = Values()\n",
        "\n",
        "# Pose with correct translation but different rotation\n",
        "pose_val1 = Pose3(Rot3.Roll(0.5), measured_translation)\n",
        "values.insert(pose_key, pose_val1)\n",
        "error1 = factor.error(values)\n",
        "print(f\"Error with correct translation: {error1} (Should be near zero)\")\n",
        "\n",
        "# Pose with incorrect translation\n",
        "pose_val2 = Pose3(Rot3.Roll(0.5), Point3(10.2, 19.9, 5.1))\n",
        "values.update(pose_key, pose_val2)\n",
        "error2 = factor.error(values)\n",
        "print(f\"Error with incorrect translation: {error2} (Should be non-zero)\")\n",
        "\n",
        "# Check that rotation change doesn't affect unwhitened error\n",
        "pose_val3 = Pose3(Rot3.Yaw(1.0), Point3(10.2, 19.9, 5.1))\n",
        "values.update(pose_key, pose_val3)\n",
        "error3 = factor.error(values)\n",
        "unwhitened2 = factor.unwhitenedError(values)\n",
        "print(f\"Error with different rotation: {error3} (Should reflect Jacobian change)\")\n",
        "print(f\"Unwhitened error with different rotation: {unwhitened2} (Should be [0.2, -0.1, 0.1])\")\n",
        "# assert np.allclose(error2, error3) # Whitened error WILL change due to Jacobian\n",
        "assert np.allclose(unwhitened2, np.array([0.2, -0.1, 0.1]))"
      ]
    }
  ],
  "metadata": {
    "kernelspec": {
      "display_name": "gtsam",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.13.1"
    }
  },
  "nbformat": 4,
  "nbformat_minor": 0
}
